import utime
from machine import Timer,PWM
from Maix import GPIO
from board import board_info
from fpioa_manager import fm
flag = 1
fm.register(30,fm.fpioa.GPIOHS14)
fm.register(31,fm.fpioa.GPIOHS15)
fm.register(32,fm.fpioa.GPIOHS16)
fm.register(33,fm.fpioa.GPIOHS17)
fm.register(34,fm.fpioa.GPIOHS18)
fm.register(35,fm.fpioa.GPIOHS19)
#上拉按下为0 下拉按下为1
H1=GPIO(GPIO.GPIOHS15,GPIO.OUT)
H2=GPIO(GPIO.GPIOHS14,GPIO.OUT)
L1=GPIO(GPIO.GPIOHS16,GPIO.PULL_UP)
L2=GPIO(GPIO.GPIOHS17,GPIO.PULL_UP)
L3=GPIO(GPIO.GPIOHS18,GPIO.PULL_UP)
L4=GPIO(GPIO.GPIOHS19,GPIO.PULL_UP)
H1.value(1)
H2.value(1)
#PWM通过定时器配置，接到IO17引脚
tim1 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim2 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
S2 = PWM(tim1, freq=50, duty=0, pin=15)   #y
S1 = PWM(tim2, freq=50, duty=0, pin=17)   #x

'''
说明：舵机控制函数
功能：180度舵机：angle:-90至90 表示相应的角度
     360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
    【duty】占空比值：0-100
'''
i_x = 0
i_y = -345
o_x = 0
o_y = 0
iiii = 0
def Servo_x(servo,angle):
    #S2.duty((angle+90)/180*10+2.5)
    S1.duty((angle+90)/180*10+2.5)

def Servo_y(servo,angle):
    S2.duty((angle+90)/180*10+2.5)
   # S1.duty((angle+90)/180*10+2.5)
Servo_x(S1,0)
Servo_y(S2,-34.5)
while(1):
    H1.value(0)
    if(L1.value()==0):  #确定原点
        o_x = i_x * 0.5
        o_y = i_y * 0.5
        print('已标记')
        print(o_x)
        print(o_y)
        utime.sleep_ms(200)
    if(L2.value()==0):  #y上
        i_y = i_y - 1
        Servo_y(S2,i_y*0.1)
        print(i_y)
        utime.sleep_ms(200)
    if(L3.value()==0):
        utime.sleep_ms(200)
    if(L4.value()==0):  #复位
        Servo_x(S1,-15)
        Servo_y(S2,-45)
        utime.sleep_ms(200)
        Servo_x(S1,0)
        Servo_y(S2,-34.5)
 #       Servo_x(S1,o_x)
  #      Servo_y(S2,o_y)
   #     print(o_x)
    #    print(o_y)
#        i_x = o_x * 2
#        i_y = o_y * 2
#        Servo_x(S1,i_x)
#        Servo_y(S2,i_y)
        print('复位')
        utime.sleep_ms(200)
    H1.value(1)
    H2.value(0)
    if(L1.value()==0):  #x左
        i_x = i_x + 1
        Servo_x(S1,0.1*i_x)
        print(i_x)
        utime.sleep_ms(200)
    if(L2.value()==0):  #y下
        i_y = i_y + 1
        Servo_y(S2,i_y*0.1)
        print(i_y)
        utime.sleep_ms(200)
    if(L3.value()==0):  #x右
        i_x = i_x - 1
        Servo_x(S1,0.1*i_x)
        print(i_x)
        utime.sleep_ms(200)
    if(L4.value()==0):
        Servo_x(S1,8.8)
        Servo_y(S2,-43.4)
        utime.sleep_ms(500)
        while iiii < 990:
            iiii = iiii + 1
            iiiii = 8.8 - 0.0192*iiii
            Servo_x(S1,iiiii)#-10.4
            utime.sleep_ms(2)
        iiii = 0
        while iiii < 1001:
            iiii = iiii + 1
            iiiii = -43.4 + 0.0169*iiii
            Servo_y(S2,iiiii)#-26.5
            utime.sleep_ms(2)
        iiii = 0
        while iiii < 980:
            iiii = iiii + 1
            iiiii = -10.4 + 0.0192*iiii
            Servo_x(S1,iiiii)#8.7
            utime.sleep_ms(2)
        iiii = 0
        while iiii < 1001:
            iiii = iiii + 1
            iiiii = -26.5 - 0.0169*iiii
            Servo_y(S2,iiiii)#-43.4
            utime.sleep_ms(2)
        iiii = 0
    H2.value(1)
